package com.lejos.carcontrol.version1;

import java.io.DataInputStream;
import java.io.DataOutputStream;

import lejos.nxt.LCD;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;

public class AndrNxtConnection {
  private DataInputStream dis;
  private DataOutputStream dos;
  private BTConnection btc;
  
  
  public void Connect(){
    LCD.clear();
    LCD.drawString("Waiting BTC...", 0, 0);
    
    
    btc = Bluetooth.waitForConnection();
    LCD.drawString("Connected", 0, 2);
    LCD.refresh();
    dis =  btc.openDataInputStream();
    dos = btc.openDataOutputStream();
  }
  
  public byte[] ReadBytes(){
    byte[] buffer = new byte[255];
    int length = btc.read(buffer, buffer.length);
    if(length == -2){
      btc.read(null, 255);
      return new byte[0];
    }
    else{
      byte[] data = new byte[length];
      for(int i=0; i<length; i++){
        data[i] = buffer[i];
      }
      return data;
    }
  }
  
  public void WriteBytes(byte[] data){
    byte[] length = {0x00, 0x00};
    length[0] =(byte)(data.length % 256);
    length[1] =(byte)(data.length / 256);
    try{
      dos.write(length, 0, length.length);
      dos.write(data, 0, data.length);
      dos.flush();
    }catch(Exception e){}
  }
  
  public void Disconnect() {
    if(btc != null){
      try{
      WriteBytes(new byte[]{(byte)255, (byte)255, (byte)255});
      Thread.sleep(100);
      dos.close();
      dis.close();
      btc.close();
      }catch(Exception e){}
    }
  }
}
